#include <iostream>
#include <map>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

#include <math_utils.h>

#if 0
int sign(double num) { return (num > 0) - (num < 0); }

// Huber loss function
Vector6d HuberLoss(const Vector6d &res, float epsilon = 5) {
  Vector6d huRes;
  for (int i = 0; i < res.rows(); i++) {
    if (fabs(res[i]) <= epsilon) {
      huRes[i] = 0.5 * res[i] * res[i];
    } else {
      huRes[i] = epsilon * (fabs(res[i]) - 0.5 * epsilon);
    }
  }
  return huRes;
}

Matrix6d Huberderivate(const Vector6d &res, float epsilon = 5) {
  Vector6d w;
  for (int i = 0; i < res.rows(); i++) {
    if (fabs(res[i]) <= epsilon) {
      w[i] = res[i];
    } else {
      w[i] = epsilon * sign(res[i]);
    }
  }

  DiagonalMatrix<double, Eigen::Dynamic> W(w);

  return W;
}
#endif

//x ~= 2nPI
bool IS_N2PI(double x, double th) {
  double ratio = x / 2*M_PI;
  ratio = ratio - int(ratio);

  if (fabs(ratio - int(ratio)) < th
   || fabs(fabs(ratio - int(ratio)) - 1) < th) {

    return true;
  } else {
    return false;
  }
}

//x ~= 2nPI +- PI
bool IS_N2PI_PI(double x, double th) {

  if (fabs(fabs(fmod(x, 2*M_PI)) - M_PI) < th) {

    return true;
  } else {
    return false;
  }
}

// Huber loss function
Matrix6d HuberWeights(const Vector6d &res, float epsilon = 1) {
  Vector6d w;
  for (int i = 0; i < res.rows(); i++) {
    if (fabs(res[i]) <= epsilon) {
      w[i] = 1;
    } else {
      w[i] = epsilon / abs(res[i]);
    }
  }

  DiagonalMatrix<double, Eigen::Dynamic> W(w);

  return W;
}

//cal dt before t
//#define DT_FIRST
//cal t before t
//#define T_FIRST

Sophus::SE3d FrankOpt(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_, float wr,
       bool IS_ALIGN_SCALE = false,
       Frank_METHOD solveMethod = DT_T_SIM)
{
  cout << "Frank method is used!" << endl;
  cout << "Rotation weight set to: " << wr << endl;

  //Shreshold for Huber
  float epsi = pt.size() * 1e-3;

  //pt * dT = T * ps
  //A  * X  = Y * B
  if (solveMethod == GAUSS_NEWTON) {
    cout << "The Gauss-Newton is used! The S will not be detect!" << endl;
    vector<int> idxs;
  
    map<int, Sophus::SE3d>::iterator it;
    if (pt.size() < ps.size()) {
      for (it = pt.begin(); it != pt.end(); it++) {
        if (ps.count(it->first)) {
          idxs.push_back(it->first);
        }
      }
    } else {
      for (it = ps.begin(); it != ps.end(); it++) {
        if (pt.count(it->first)) {
          idxs.push_back(it->first);
        }
      }
    }

    int iterNum = 100;

    VectorXd vweights(6);
    vweights << 1, 1, 1, wr, wr, wr;

    DiagonalMatrix<double, Eigen::Dynamic> MWeight(vweights);

    Sophus::Sim3d dTe = Sophus::Sim3d(Matrix4d::Identity());
    Sophus::Sim3d Te = Sophus::Sim3d(Matrix4d::Identity());

    for (int iter = 0; iter < iterNum; iter++) {
      Matrix13d H = Matrix13d::Zero();
      Vector13d b = Vector13d::Zero();
      double cost = 0;
      for(int i = 0; i < idxs.size(); i++) {

        Matrix4d T2 = Sophus::SE3d(pt[idxs[i]].so3().inverse(),
                                   pt[idxs[i]].translation()).matrix();
        Matrix4d T1 = Sophus::SE3d(ps[idxs[i]].so3().inverse(),
                                   ps[idxs[i]].translation()).matrix();
        Vector7d res0 = Sophus::Sim3d((T2*dTe.matrix()).inverse() * Te.matrix() * T1).log();
        Vector6d res = res0.segment(0, 6);

        Matrix6d W = HuberWeights(res, epsi);

        Sophus::Sim3d Ty = Sophus::Sim3d(T2*dTe.matrix()).inverse();
        Matrix3d Ry = Ty.rotationMatrix();
        Vector3d ty = Ty.translation();
        float sy = Ty.scale();

        Sophus::Sim3d Tx = dTe.inverse();
        Matrix3d Rx = Tx.rotationMatrix();
        Vector3d tx = Tx.translation();
        float sx = Tx.scale();

 
        Matrix<double, 6, 13> J;
  
        Matrix<double, 6, 7> J_T;
        J_T.setZero();
        J_T.block(0,0,3,3) = sy*Ry;
        J_T.block(0,3,3,3) = Sophus::SO3d::hat(ty)*Ry;
        J_T.block(3,3,3,3) = Ry;
        J_T.block(0,6,3,1) = -ty + tx;
  
        Matrix<double, 6, 6> J_dT;
        J_dT.setZero();
        J_dT.block(0,0,3,3) = sx*Rx;
        J_dT.block(0,3,3,3) = Sophus::SO3d::hat(tx)*Rx;
        J_dT.block(3,3,3,3) = Rx;
  
        J.block(0, 0, 6, 7) = J_T;
        J.block(0, 7, 6, 6) = -J_dT;

        H += J.transpose() * MWeight *W* J;
        b += -J.transpose() * MWeight *W* res;
        cost += res.transpose() * res;
      }

      /******* nullspace test
      if (iter == 0) {
        JacobiSVD<MatrixXd> svd(H, ComputeThinU | ComputeThinV);
        //cout << svd.singularValues() << endl;
        VectorXd V = svd.singularValues();
        int nullNum = 0;
        for (int i = 0; i < V.size(); i++) {
          if (V[i] < 1e-4) { nullNum++; }
        }

        cout << "V: " << V.transpose() << endl;
        cout << "nulls is " << nullNum << endl;
      }
      ***********/

      Vector13d update = H.ldlt().solve(b);
      if (std::isnan(update[0]) /* || update.norm() < 1e-3 */) {
        cout << "ERROR! update is nan" << endl;
        break;
      }

      //sophus中的scale合理阈值为e-12
      if (update[12] < -12) {
        update[12] = 0;
      }

      Te = Sophus::Sim3d::exp(update.segment(0,7)) * Te;

      Vector7d dTs;
      dTs << update.segment(7,6), update.segment(6, 1);
      dTe = Sophus::Sim3d::exp(dTs) * dTe;

      if (update.norm() < 1e-4) {
        // converge
        break;
      }

      cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    s_ = Te.scale();
    R_ = Te.rotationMatrix();
    t_ = Te.translation();

    S_ = 1;

    return Sophus::SE3d(dTe.rotationMatrix(), dTe.translation());
  }

  Vector3d dt(0,0,0);
  Matrix3d SS = Matrix3d::Identity();

  int match_num = 0;
  //match ids
  vector<int> idxs;

  Sophus::SO3d al0, be0;
  bool firstInit = false;
  Matrix3d W = Matrix3d::Zero();
  double M = 0;
  map<int, Sophus::SE3d>::iterator it;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {

        if (!firstInit) {
          al0 = it->second.so3();
          be0 = ps[it->first].so3();
          firstInit = true;
        }

        Vector3d alpha = (al0.inverse() * it->second.so3()).log();
        Vector3d belta = (be0.inverse() * ps[it->first].so3()).log();
        idxs.push_back(it->first);

        W = W + alpha * belta.transpose();
        M = M + belta.transpose() * belta;

        match_num++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {

        if (!firstInit) {
          al0 = pt[it->first].so3();
          be0 = it->second.so3();
          firstInit = true;
        }

        Vector3d alpha = (al0.inverse() * pt[it->first].so3()).log();
        Vector3d belta = (be0.inverse() * it->second.so3()).log();
        idxs.push_back(it->first);

        W = W + alpha * belta.transpose();
        M = M + belta.transpose() * belta;
        match_num++;
      }
    }
  }

  // SVD on W
  JacobiSVD<Matrix3d> svd(W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  R_ = U * (V.transpose());

  Vector3d ts0 = ps[idxs[0]].translation();
  Vector3d tt0 = pt[idxs[0]].translation();

  if (solveMethod == T_FIRST) {
    cout << "calulate t first" << endl;
    //the dirft will be large with the runtime.
    //use the first cal_num to calculate
    int cal_num = match_num;
    //int cal_num = 20;
  
    Matrix<double, Dynamic, 4> A(cal_num * 3, 4);
    Matrix<double, Dynamic, 1> b(cal_num * 3, 1);
  
    //the first maybe equals: 0 = 0
    for (int i = 1; i <= cal_num; i++) {
      Matrix3d Rt = pt[idxs[i]].so3().matrix();
      Matrix3d Rs = ps[idxs[i]].so3().matrix();
      Vector3d ts = ps[idxs[i]].translation();
      Vector3d tt = pt[idxs[i]].translation();
  
      A.block((i-1)*3, 0, 3, 1) = R_ * (be0.matrix().transpose()*Rs*ts - ts0);
      A.block((i-1)*3, 1, 3, 3) = al0.matrix().transpose()*Rt - Matrix3d::Identity();
  
      b.segment((i-1)*3, 3) = al0.matrix().transpose()*Rt*tt - tt0;
    }
  
    Vector4d st = A.fullPivHouseholderQr().solve(b);
  
    s_ = st[0];
  
    t_ = st.segment(1, 3);
  
    if (s_ < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = -SS * R_;
      s_ *= -1;
    }
  
    if(R_.determinant() < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = SS * R_;
    }
  
    if (!IS_ALIGN_SCALE) {
      s_ = 1;
  
      Matrix<double, Dynamic, 3> At(cal_num * 3, 3);
      Matrix<double, Dynamic, 1> bt(cal_num * 3, 1);
    
      //the first maybe equals: 0 = 0
      for (int i = 1; i <= cal_num; i++) {
        Matrix3d Rt = pt[idxs[i]].so3().matrix();
        Matrix3d Rs = ps[idxs[i]].so3().matrix();
        Vector3d ts = ps[idxs[i]].translation();
        Vector3d tt = pt[idxs[i]].translation();
    
        At.block((i-1)*3, 0, 3, 3) = al0.matrix().transpose()*Rt - Matrix3d::Identity();
    
        bt.segment((i-1)*3, 3) = al0.matrix().transpose()*Rt*tt - tt0
                              - SS*R_*(be0.matrix().transpose()*Rs*ts - ts0);
      }
    
      t_ = At.fullPivHouseholderQr().solve(bt);
    }
  
    dt = al0.matrix() * (s_ * SS * R_ * ts0 + t_ - tt0);

  } else if (solveMethod == DT_FIRST) {

    cout << "calulate dt first" << endl;
    //the dirft will be large with the runtime.
    //use the first cal_num to calculate
    int cal_num = match_num;
    //int cal_num = 20;

    Matrix<double, Dynamic, 4> A(cal_num * 3, 4);
    Matrix<double, Dynamic, 1> b(cal_num * 3, 1);

    //the first maybe equals: 0 = 0
    for (int i = 1; i <= cal_num; i++) {
      Sophus::SO3d Rt = pt[idxs[i]].so3();
      Vector3d ts = ps[idxs[i]].translation();
      Vector3d tt = pt[idxs[i]].translation();

      A.block((i-1)*3, 0, 3, 1) = R_ * (ts - ts0);
      A.block((i-1)*3, 1, 3, 3) = al0.matrix().transpose() - Rt.matrix().transpose();

      b.segment((i-1)*3, 3) = tt - tt0;
    }

    Vector4d sDt = A.fullPivHouseholderQr().solve(b);

    s_ = sDt[0];
    dt = sDt.segment(1, 3);

    if (s_ < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = -SS * R_;
      s_ *= -1;
    }

    if(R_.determinant() < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = SS * R_;
    }

    if (!IS_ALIGN_SCALE) {
      s_ = 1;

      Matrix<double, Dynamic, 3> At(cal_num * 3, 3);
      Matrix<double, Dynamic, 1> bt(cal_num * 3, 1);
    
      //the first maybe equals: 0 = 0
      for (int i = 1; i <= cal_num; i++) {
        Sophus::SO3d Rt = pt[idxs[i]].so3();
        Vector3d ts = ps[idxs[i]].translation();
        Vector3d tt = pt[idxs[i]].translation();
    
        At.block((i-1)*3, 0, 3, 3) = al0.matrix().transpose() - Rt.matrix().transpose();
    
        bt.segment((i-1)*3, 3) = tt - tt0 - SS*R_*(ts-ts0);
      }
    
      dt = At.fullPivHouseholderQr().solve(bt);
    }

    t_ = -(s_ * SS * R_ * ts0 - tt0 - al0.matrix().transpose() * dt);

  } else {
  
    cout << "calulate t & dt simulataneously" << endl;
    int cal_num = match_num;
    //int cal_num = 10;
  
    Matrix<double, Dynamic, 7> A(cal_num * 3, 7);
    Matrix<double, Dynamic, 1> b(cal_num * 3, 1);
  
    //the first maybe equals: 0 = 0
    for (int i = 0; i < cal_num; i++) {
      Sophus::SO3d Rt = pt[idxs[i]].so3();
      Vector3d ts = ps[idxs[i]].translation();
      Vector3d tt = pt[idxs[i]].translation();
  
      A.block(i*3, 0, 3, 1) = R_ * ts;
      A.block(i*3, 1, 3, 3) = -Rt.matrix().transpose();
      A.block(i*3, 4, 3, 3) = Matrix3d::Identity();
  
      b.segment(i*3, 3) = tt;
    }
  
    //sDtt:[s, dt, t]
    VectorXd sDtt = A.fullPivHouseholderQr().solve(b);

    //Eigen::FullPivLU<Eigen::MatrixXd> lu(A);
    //int rank = lu.rank();
    //cout << "rank: " << rank << endl;

  
    s_ = sDtt[0];
    dt = sDtt.segment(1, 3);
    t_ = sDtt.segment(4, 3);
  
    if (s_ < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = -SS * R_;
      s_ *= -1;
    }

    if(R_.determinant() < 0) {
      SS(2, 2) = -1;
      S_ = -1;
      R_ = SS * R_;
    }

    if (!IS_ALIGN_SCALE) {
      s_ = 1;

      Matrix<double, Dynamic, 6> At(cal_num * 3, 6);
      Matrix<double, Dynamic, 1> bt(cal_num * 3, 1);
    
      //the first maybe equals: 0 = 0
      for (int i = 0; i < cal_num; i++) {
        Sophus::SO3d Rt = pt[idxs[i]].so3();
        Vector3d ts = ps[idxs[i]].translation();
        Vector3d tt = pt[idxs[i]].translation();
    
        At.block(i*3, 0, 3, 3) = -Rt.matrix().transpose();
        At.block(i*3, 3, 3, 3) = Matrix3d::Identity();
    
        bt.segment(i*3, 3) = tt - SS*R_*ts;
      }
    
      VectorXd dtt = At.fullPivHouseholderQr().solve(bt);

      dt = dtt.segment(0, 3);
      t_ = dtt.segment(3, 3);
    }
  }

  Matrix3d dR = SS * al0.matrix() * SS * R_ * be0.matrix().transpose();

  return Sophus::SE3d(Sophus::SO3d(dR), dt);
}

//Params: (pt,ps,s,R,t), Pt = s*R*ps + t;
Sophus::Sim3d pose_estimation_3d3d_use_salas(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       float wr,
       bool IS_ROT_TRANSPOSE,
       bool IS_ALIGN_SCALE)
{
  cout << "salas (withot S) is used!" << endl;
  cout << "Rotation weight set to: " << wr << endl;

  map<int, Sophus::SE3d>::iterator it;
  vector<Sophus::SE3d> Qs, Ps;

  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        Qs.push_back(it->second);
        Ps.push_back(ps[it->first]);
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        Ps.push_back(it->second);
        Qs.push_back(pt[it->first]);
      }
    }
  }

  VectorXd vweights(7);
  vweights << 1, 1, 1, wr, wr, wr, 1;

  DiagonalMatrix<double, Eigen::Dynamic> MWeight(vweights);

  const int iterations = 50; 
  Sophus::Sim3d T = Sophus::Sim3d(Matrix4d::Identity());
  for (int iter = 0; iter < iterations; iter++) {
    Matrix7d H = Matrix7d::Zero();
    Vector7d b = Vector7d::Zero();
    double cost = 0;
    for(int i = 0; i < Ps.size(); i++) {
      Eigen::Matrix3d Rq, Rp;
      if (IS_ROT_TRANSPOSE) {
        Rq = Qs[i].so3().matrix().transpose();
        Rp = Ps[i].so3().matrix().transpose();
      } else {
        Rq = Qs[i].so3().matrix();
        Rp = Ps[i].so3().matrix();
      }

      float s = T.scale();
      Sophus::Sim3d Q(Sophus::RxSO3d(s, Rq), Qs[i].translation());
      Sophus::Sim3d P(Sophus::RxSO3d(1, Rp), Ps[i].translation());
      Vector7d res = (Q.inverse() * T * P).log();

      Matrix3d Rt = Q.inverse().rotationMatrix();
      Vector3d tt = Q.inverse().translation();

      Matrix7d J_T;
      J_T.setZero();
      J_T.block(0,0,3,3) = Rt/s;
      J_T.block(0,3,3,3) = Sophus::SO3d::hat(tt)*Rt;
      J_T.block(0,6,3,1) = -tt;
      J_T.block(3,3,3,3) = Rt; 
      //J_T.coeffRef(6, 6) = 1;


      H += J_T.transpose() * MWeight * J_T;
      b += -J_T.transpose() * MWeight* res;
      cost += res.transpose() * res;
    }   

    Vector7d update = H.ldlt().solve(b);
    if (std::isnan(update[0]) /* || update.norm() < 1e-3 */) {
      cout << "ERROR! update is nan" << endl;
      break;
    }   

    if (update[6] < -12) {
      update[6] = 0;
    }

    T = Sophus::Sim3d::exp(update) * T;
    if(!IS_ALIGN_SCALE) {
      T.setScale(1);
      VectorXd u = update.segment(1, 6);
      if (u.norm() < 1e-4) {
        // converge
        break;
      }
    }

    if (update.norm() < 1e-4) {
      // converge
      break;
    }   

    cout << "iteration: " << iter << ", cost: " << cost << endl;
  }

  return T;
}


//if IS_ALIGN_SCALE is false, s_ will be 1
//S is for delta(R) < 0, which means the pt & ps were in different frame,
//one is under left-hand frame & the other is under right-hand frame
void pose_estimation_3d3d_use_Umeyama(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE = false)
{
  cout << "Umeyama method is used!" << endl;

  Vector3d pt_ave(0,0,0), ps_ave(0,0,0);     // center of mass

  map<int, Sophus::SE3d>::iterator it;
  int match_num = 0;

  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        pt_ave += it->second.translation();
        ps_ave += ps[it->first].translation();
        match_num++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        ps_ave += it->second.translation();
        pt_ave += pt[it->first].translation();
        match_num++;
      }
    }
  }

  pt_ave = Vector3d(pt_ave / match_num);
  ps_ave = Vector3d(ps_ave / match_num);

  vector<Vector3d> qt(match_num), qs(match_num); // remove the center
  int i = 0;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        qt[i] = it->second.translation() - pt_ave;
        qs[i] = ps[it->first].translation() - ps_ave;
        i++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        qs[i] = it->second.translation() - ps_ave;
        qt[i] = pt[it->first].translation() - pt_ave;
        i++;
      }
    }
  }

  // compute q1*qs^T
  Matrix3d W = Matrix3d::Zero();
  double M = 0;
  for (int i = 0; i < match_num; i++) {
    W = W + Vector3d(qt[i][0], qt[i][1], qt[i][2])
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose();
    M += Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose()
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]);
  }

  // SVD on W
  JacobiSVD<Matrix3d> svd (W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  Matrix3d SS = Matrix3d::Identity();
  if (U.determinant() * V.determinant() < 0) {
    SS(2, 2) = -1;
  }

  R_ = U * SS * (V.transpose());
  if (IS_ALIGN_SCALE) {
    s_ = (R_.transpose() * W).trace() / M;
  } else {
    s_ = 1;
  }

  t_ = Vector3d(pt_ave[0], pt_ave[1], pt_ave[2])
    - s_ * R_ * Vector3d(ps_ave[0], ps_ave[1], ps_ave[2]);
}

//Params: (pt,ps,s,R,t), Pt = s*R*ps + t;
void pose_estimation_3d3d(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_,
       bool IS_ALIGN_SCALE)
{
  cout << "Umeyama (withot S) is used!" << endl;

  map<int, Sophus::SE3d>::iterator it;
  int match_num = 0;
  vector<Vector3d> qt, qs;

  int first_id = 0;
  bool first_id_init = false;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }
        qt.push_back(it->second.translation() - pt[first_id].translation());
        qs.push_back(ps[it->first].translation() - ps[first_id].translation());
        match_num++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }
        qs.push_back(it->second.translation() - ps[first_id].translation());
        qt.push_back(pt[it->first].translation() - pt[first_id].translation());
        match_num++;
      }
    }
  }

  Matrix3d W = Matrix3d::Zero();
  double M = 0;
  for (int i=0; i < match_num; i++) {
    W = W + Vector3d(qt[i][0], qt[i][1], qt[i][2])
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose();
    M += Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose()
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]);
  }
  //cout<<"W="<<W<<endl;

  // SVD on W
  JacobiSVD<Matrix3d> svd(W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  Matrix3d SS = Matrix3d::Identity();
  if (U.determinant() * V.determinant() < 0) {
    SS(2, 2) = -1;
  }

  R_ = U * SS * (V.transpose());
  if (IS_ALIGN_SCALE) {
    s_ = (R_.transpose() * W).trace() / M;
  } else {
    s_ = 1;
  }

  t_ = pt[first_id].translation() - s_ * R_ * ps[first_id].translation();
}

//Params: (pt,ps,s,S,R,t), Pt = s*S*R*ps + t;
void pose_estimation_3d3d(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE)
{
  cout << "ROAM method(without rotation) is used!" << endl;

  map<int, Sophus::SE3d>::iterator it;
  int match_num = 0;
  vector<Vector3d> qt, qs;

  int first_id = 0;
  bool first_id_init = false;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }
        qt.push_back(it->second.translation() - pt[first_id].translation());
        qs.push_back(ps[it->first].translation() - ps[first_id].translation());
        match_num++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }
        qs.push_back(it->second.translation() - ps[first_id].translation());
        qt.push_back(pt[it->first].translation() - pt[first_id].translation());
        match_num++;
      }
    }
  }

  Matrix3d W = Matrix3d::Zero();
  double M = 0;
  for (int i=0; i < match_num; i++) {
    W = W + Vector3d(qt[i][0], qt[i][1], qt[i][2])
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose();
    M += Vector3d(qs[i][0], qs[i][1], qs[i][2]).transpose()
          * Vector3d(qs[i][0], qs[i][1], qs[i][2]);
  }

  // SVD on W
  JacobiSVD<Matrix3d> svd(W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  Matrix3d SS = Matrix3d::Identity();
  if (U.determinant() * V.determinant() < 0) {
    S_ = -1;
    SS(2, 2) = -1;
  }

  R_ = U * (V.transpose());
  if (IS_ALIGN_SCALE) {
    s_ = (R_.transpose() * W).trace() / M;
  } else {
    s_ = 1;
  }

  //R_ = U * SS * (V.transpose());
  R_ = SS * R_;
  t_ = pt[first_id].translation() - s_ * SS * R_ * ps[first_id].translation();
}

//Params: (pt,ps,s,S,R,t), Pt = s*SR*ps + St; Rt1^T * Rt = SR *Rs1^T *Rs* SR
//trans: rot 1:1
#if 0
void pose_estimation_3d3d_with_rot(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE)
{
  map<int, Sophus::SE3d>::iterator it;
  int match_num = 0;
  //trans and rots
  vector<Vector3d> qt, qs;
  //only trans
  vector<Vector3d> tt, ts;

  Sophus::SO3d al0 = pt.begin()->second.so3();
  Sophus::SO3d be0 = ps.begin()->second.so3();

  int first_id = 0;
  bool first_id_init = false;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }

        qt.push_back(it->second.translation() - pt[first_id].translation());
        qs.push_back(ps[it->first].translation() - ps[first_id].translation());
        tt.push_back(it->second.translation() - pt[first_id].translation());
        ts.push_back(ps[it->first].translation() - ps[first_id].translation());

        Vector3d alpha = (al0.inverse() * it->second.so3()).log();
        qt.push_back(alpha);
        Vector3d belta = (be0.inverse() * ps[it->first].so3()).log();
        qs.push_back(belta);

        match_num++;
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;
        }

        qs.push_back(it->second.translation() - ps[first_id].translation());
        qt.push_back(pt[it->first].translation() - pt[first_id].translation());
        ts.push_back(it->second.translation() - ps[first_id].translation());
        tt.push_back(pt[it->first].translation() - pt[first_id].translation());

        Vector3d alpha = (al0.inverse() * pt[it->first].so3()).log();
        qt.push_back(alpha);
        Vector3d belta = (be0.inverse() * it->second.so3()).log();
        qs.push_back(belta);

        match_num++;
      }
    }
  }

  Matrix3d Wt = Matrix3d::Zero();
  double Mt = 0;
  for (int i=0; i < match_num; i++) {
    Wt = Wt + Vector3d(tt[i][0], tt[i][1], tt[i][2])
          * Vector3d(ts[i][0], ts[i][1], ts[i][2]).transpose();
    Mt += Vector3d(ts[i][0], ts[i][1], ts[i][2]).transpose()
          * Vector3d(ts[i][0], ts[i][1], ts[i][2]);
  }

  JacobiSVD<Matrix3d> svd(W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  Matrix3d SS = Matrix3d::Identity();
  if (U.determinant() * V.determinant() < 0) {
    SS(2, 2) = -1;
  }

  R_ = U * SS * (V.transpose());

  if (IS_ALIGN_SCALE) {
    s_ = (R_.transpose() * Wt).trace() / Mt;
  } else {
    s_ = 1;
  }

  Matrix3d W = Matrix3d::Zero();
  for (int i=0; i < match_num; i++) {
    W = Wt + Vector3d(qt[i+match_num][0], qt[i+match_num][1], qt[i+match_num][2])
          * Vector3d(qs[i+match_num][0],
                            qs[i+match_num][1], qs[i+match_num][2]).transpose()/s_;
  }

  // SVD on W
  JacobiSVD<Matrix3d> svd1(W, ComputeThinU|ComputeThinV);
  Matrix3d U1 = svd1.matrixU();
  Matrix3d V1 = svd1.matrixV();

  Matrix3d SS = Matrix3d::Identity();
  if (U1.determinant() * V1.determinant() < 0) {
    S_ = -1;
    SS(2, 2) = -1;
  }

  R_ = U1 * (V1.transpose());

  //R_ = U * SS * (V.transpose());
  R_ = SS * R_;
  t_ = SS * (pt[first_id].translation() - s_ * SS * R_ * ps[first_id].translation());
}
#endif

//weights, trans: rot 1:s
void pose_estimation_3d3d_with_rot(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE,
       float wr,
       bool IS_ROT_TRANSPOSE)
{
  cout << "ROAM method(consider rotation) is used!" << endl;

  //when rot < nr then this data consider to be unreliable
  //float nr = 0.05;
  //when U do not need this make nr be 0
  float nr = 0;

  //weights for rot constraint
  //float wr = 5;
  //ignore rotation data, then set wr to 0
  //float wr = 0;

  map<int, Sophus::SE3d>::iterator it;

  Sophus::SO3d al0 = pt.begin()->second.so3();
  Sophus::SO3d be0 = ps.begin()->second.so3();

  Vector3d alRec, beRec;
  
  Matrix3d Wt = Matrix3d::Zero();
  double Mt = 0;
  Matrix3d Wr = Matrix3d::Zero();

  int first_id = 0;
  bool first_id_init = false;
  if (pt.size() < ps.size()) {
    for (it = pt.begin(); it != pt.end(); it++) {
      if (ps.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;

          continue;
        }

        Vector3d qt10 = it->second.translation() - pt[first_id].translation();
        Vector3d qs10 = ps[it->first].translation() - ps[first_id].translation();

        Wt += qt10 * qs10.transpose();
        Mt += qs10.transpose() * qs10;

        Vector3d alpha, belta;
        if (IS_ROT_TRANSPOSE) {
          alpha = (al0.inverse() * it->second.so3()).log();
          belta = (be0.inverse() * ps[it->first].so3()).log();
        } else {
          alpha = (it->second.so3() * al0.inverse()).log();
          belta = (ps[it->first].so3() * be0.inverse()).log();
        }

        if (alpha.norm() < nr || belta.norm() < nr) { continue; }
 
        static int sign = 1;
        double sa = (alpha.normalized() + alRec.normalized()).norm();
        if (alRec.norm() > 3 && sa < 0.5) {
          sign *= -1;
        }
        double sb = (belta.normalized() + beRec.normalized()).norm();
        if (beRec.norm() > 3 && sb < 0.5) {
          sign *= -1;
        }


#if 0
        double p = fabs(fmod(alpha.norm() + belta.norm(), 2*M_PI));
        double m = fabs(fmod(alpha.norm() - belta.norm(), 2*M_PI));
        int sign = 1;
        if (p < m) {
          sign = -1;
        }
#endif

        Wr += sign * alpha * belta.transpose();
        //Wr += sign * alpha/alpha.norm() * belta.transpose()/belta.norm();
        //Wr += sign * alpha.normalized() * belta.normalized().transpose();
      }
    }
  } else {
    for (it = ps.begin(); it != ps.end(); it++) {
      if (pt.count(it->first)) {
        if (!first_id_init) {
          first_id = it->first;
          first_id_init = true;

          continue;
        }

        Vector3d qs10 = it->second.translation() - ps[first_id].translation();
        Vector3d qt10 = pt[it->first].translation() - pt[first_id].translation();

        Wt += qt10 * qs10.transpose();
        Mt += qs10.transpose() * qs10;

        Vector3d alpha, belta;
        if (IS_ROT_TRANSPOSE) {
          alpha = (al0.inverse() * pt[it->first].so3()).log();
          belta = (be0.inverse() * it->second.so3()).log();
        } else {
          alpha = (pt[it->first].so3() * al0.inverse()).log();
          belta = (it->second.so3() * be0.inverse()).log();
        }


       if (alpha.norm() < nr || belta.norm() < nr) { continue; }

       static int sign = 1;
       double sa = (alpha.normalized() + alRec.normalized()).norm();
       if (alRec.norm() > 3 && sa < 0.5) {
         sign *= -1;
       }
       double sb = (belta.normalized() + beRec.normalized()).norm();
       if (beRec.norm() > 3 && sb < 0.5) {
         sign *= -1;
       }


#if 0
        double p = fabs(alpha.norm() + belta.norm());
        p = fabs(p - round(p / (2*M_PI)) * 2*M_PI);
        double m = fabs(alpha.norm() - belta.norm());
        m = fabs(m - round(m / (2*M_PI)) * 2*M_PI);

        int sign = 1;
        if (p < m) {
          cout << "sign -1" << endl;
          sign = -1;
        }
#endif
        Wr += sign * alpha * belta.transpose();
        //Wr += sign * alpha/alpha.norm() * belta.transpose()/belta.norm();
        //Wr += sign * alpha.normalized() * belta.normalized().transpose();

        //if (alpha.norm() < 2.7) {
        //  Wr += sign * alpha * belta.transpose();
        //}

        alRec = alpha;
        beRec = belta;
      }
    }
  }

  Wr *= wr;

  if (Wr.determinant() == 0) {
    cout << "WARINING!!! There is no data in Wr, maybe the \'nr\' parameters"
    " need to be reset based on trajectory noise" << endl;
    cout << "Or the weight for rotation is set to be 0!" << endl;

    Wr = Wt;
  }

  JacobiSVD<Matrix3d> svdr = Wr.jacobiSvd(ComputeThinU|ComputeThinV);
  Matrix3d Ur = svdr.matrixU();
  Matrix3d Vr = svdr.matrixV();
  Matrix3d Rr = Ur * Vr.transpose();

  JacobiSVD<Matrix3d> svdt = Wt.jacobiSvd(ComputeThinU|ComputeThinV);
  Matrix3d Ut = svdt.matrixU();
  Matrix3d Vt = svdt.matrixV();
  Matrix3d Rt = Ut * Vt.transpose();

  Matrix3d S = Matrix3d::Identity();
  S(2, 2) = -1;

  double ME = (Rr - Rt).norm();
  double MES = (-S*Rr - Rt).norm();

  Matrix3d SS = Matrix3d::Identity();
  //means Rr = -SR
  if (ME < MES) {
    Wt += Wr;
  //means Rr = R, or the rotation is tiny
  } else {
    S_ = -1;
    SS(2, 2) = -1;
    Wt -= Wr;
  }
 
  JacobiSVD<Matrix3d> svd = (Wt).jacobiSvd(ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  if (S_ * U.determinant() * V.determinant() < 0) {
    R_ = SS * U * S * (V.transpose());
  } else {
    R_ = SS * U * (V.transpose());
  }

  if (IS_ALIGN_SCALE) {
    s_ = ((SS*R_).transpose() * Wt).trace() / Mt;
  } else {
    s_ = 1;
  }

  t_ = pt[first_id].translation() - s_ * SS * R_ * ps[first_id].translation();
}


Quaterniond Quaternion_S_lerp(Quaterniond &start_q,
       Quaterniond &end_q, double c)
{
  Quaterniond lerp_q;

  double cos_angle = start_q.x() * end_q.x()
                   + start_q.y() * end_q.y()
                   + start_q.z() * end_q.z()
                   + start_q.w() * end_q.w();

  // If the dot product is negative, the quaternions have opposite handed-ness and slerp won't take
  // the shorter path. Fix by reversing one quaternion.
  if (cos_angle < 0) {
          end_q.x() = -end_q.x();
          end_q.y() = -end_q.y();
          end_q.z() = -end_q.z();
          end_q.w() = -end_q.w();
          cos_angle = -cos_angle;
  }

  double ratio_A, ratio_B;

   //If the inputs are too close for comfort, linearly interpolate
  if (cos_angle > 0.99995f) {
          ratio_A = 1.0f - c;
          ratio_B = c;
  }
  else {
          double sin_angle = sqrt( 1.0f - cos_angle * cos_angle);
          double angle = atan2(sin_angle, cos_angle);
          ratio_A = sin((1.0f - c) * angle)  / sin_angle;
          ratio_B = sin(c * angle) / sin_angle;
  }

  lerp_q.x() = ratio_A * start_q.x() + ratio_B * end_q.x();
  lerp_q.y() = ratio_A * start_q.y() + ratio_B * end_q.y();
  lerp_q.z() = ratio_A * start_q.z() + ratio_B * end_q.z();
  lerp_q.w() = ratio_A * start_q.w() + ratio_B * end_q.w();

  return lerp_q.normalized();
}

Sophus::SE3d se_lerp(Sophus::SE3d se1, Sophus::SE3d se2, int c)
{
  //Vector3d p1(se1.translation()[0], se1.translation()[1], se1.translation()[2]);
  //Vector3d p2(se2.translation()[0], se2.translation()[1], se2.translation()[2]);
  Vector3d p1 = se1.translation();
  Vector3d p2 = se2.translation();
  Quaterniond q1 = se1.so3().unit_quaternion();
  Quaterniond q2 = se2.so3().unit_quaternion();

  Quaterniond q3 = Quaternion_S_lerp(q1, q2, c);
  Vector3d p3 = c * p1 + (1-c) * p2;
  return Sophus::SE3d(q3, p3);
}


